<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of pf</title>
  <meta name="keywords" content="pf">
  <meta name="description" content="PF  Generic Particle Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; pf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>pf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>PF  Generic Particle Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [estimate, ParticleFilterDS, pNoise, oNoise] = pf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> PF  Generic Particle Filter

   This filter is also known as the 'Bootstrap Particle Filter' or the 'Condensation Algorithm'

   [estimate, ParticleFilterDS, pNoise, oNoise] = PF(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         ParticleFilterDS     Particle filter data structure. Contains set of particles as well as their corresponding
                              weights.
         pNoise               process noise data structure
         oNoise               observation noise data structure
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          Inference data structure generated by GENINFDS function.

   OUTPUT
         estimate             State estimate generated from posterior distribution of state given all observation. Type of
                              estimate is specified by 'InferenceDS.estimateType'
         ParticleFilterDS     Updated Particle filter data structure. Contains set of particles as well as their corresponding weights.
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

   ParticleFilterDS fields:
         .N                   (scalar) number of particles
         .particles           (statedim-by-N matrix) particle buffer
         .weights             (1-by-N r-vector) particle weights

   Required InferenceDS fields:
         .estimateType        (string) Estimate type : 'mean', 'mode', etc.
         .resampleThreshold   (scalar) If the ratio of the 'effective particle set size' to the total number of particles
                                       drop below this threshold  i.e.  (N_efective/N) &lt; resampleThreshold
                                       the particles will be resampled.  (N_efective is always less than or equal to N)

   See also
   <a href="kf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">KF</a>, <a href="ekf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">EKF</a>, <a href="ukf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">UKF</a>, <a href="cdkf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">CDKF</a>, <a href="srukf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">SRUKF</a>, <a href="srcdkf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">SRCDKF</a>
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>	CVECREP  Column vector replicate</li><li><a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>	RESIDUALRESAMPLE  Residual resampling for SIR. Performs the resampling stage of</li><li><a href="rvecrep.html" class="code" title="function m = rvecrep(v,c)">rvecrep</a>	RVECREP  Row vector replicate</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse4.html" class="code" title="">demse4</a>	DEMSE4  Bearing Only Tracking Example</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse5.html" class="code" title="">demse5</a>	DEMSE4  Bearing and Frequency Tracking Example</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [estimate, ParticleFilterDS, pNoise, oNoise] = pf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% PF  Generic Particle Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   This filter is also known as the 'Bootstrap Particle Filter' or the 'Condensation Algorithm'</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   [estimate, ParticleFilterDS, pNoise, oNoise] = PF(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0010 <span class="comment">%</span>
0011 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0012 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0013 <span class="comment">%</span>
0014 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0015 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0016 <span class="comment">%   system.</span>
0017 <span class="comment">%</span>
0018 <span class="comment">%   INPUT</span>
0019 <span class="comment">%         ParticleFilterDS     Particle filter data structure. Contains set of particles as well as their corresponding</span>
0020 <span class="comment">%                              weights.</span>
0021 <span class="comment">%         pNoise               process noise data structure</span>
0022 <span class="comment">%         oNoise               observation noise data structure</span>
0023 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0024 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0025 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0026 <span class="comment">%         InferenceDS          Inference data structure generated by GENINFDS function.</span>
0027 <span class="comment">%</span>
0028 <span class="comment">%   OUTPUT</span>
0029 <span class="comment">%         estimate             State estimate generated from posterior distribution of state given all observation. Type of</span>
0030 <span class="comment">%                              estimate is specified by 'InferenceDS.estimateType'</span>
0031 <span class="comment">%         ParticleFilterDS     Updated Particle filter data structure. Contains set of particles as well as their corresponding weights.</span>
0032 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0033 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0034 <span class="comment">%</span>
0035 <span class="comment">%   ParticleFilterDS fields:</span>
0036 <span class="comment">%         .N                   (scalar) number of particles</span>
0037 <span class="comment">%         .particles           (statedim-by-N matrix) particle buffer</span>
0038 <span class="comment">%         .weights             (1-by-N r-vector) particle weights</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Required InferenceDS fields:</span>
0041 <span class="comment">%         .estimateType        (string) Estimate type : 'mean', 'mode', etc.</span>
0042 <span class="comment">%         .resampleThreshold   (scalar) If the ratio of the 'effective particle set size' to the total number of particles</span>
0043 <span class="comment">%                                       drop below this threshold  i.e.  (N_efective/N) &lt; resampleThreshold</span>
0044 <span class="comment">%                                       the particles will be resampled.  (N_efective is always less than or equal to N)</span>
0045 <span class="comment">%</span>
0046 <span class="comment">%   See also</span>
0047 <span class="comment">%   KF, EKF, UKF, CDKF, SRUKF, SRCDKF</span>
0048 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0049 <span class="comment">%</span>
0050 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0051 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0052 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0053 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0054 <span class="comment">%</span>
0055 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0056 <span class="comment">%   detail.</span>
0057 
0058 <span class="comment">%=============================================================================================</span>
0059 
0060 
0061 <span class="keyword">if</span> (nargin ~= 7) error(<span class="string">' [ pf ] Incorrect number of input arguments.'</span>); <span class="keyword">end</span>
0062 
0063 <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0064 
0065 Xdim  = InferenceDS.statedim;                            <span class="comment">% extract state dimension</span>
0066 Odim  = InferenceDS.obsdim;                              <span class="comment">% extract observation dimension</span>
0067 U1dim = InferenceDS.U1dim;                               <span class="comment">% extract exogenous input 1 dimension</span>
0068 U2dim = InferenceDS.U2dim;                               <span class="comment">% extract exogenous input 2 dimension</span>
0069 Vdim  = InferenceDS.Vdim;                                <span class="comment">% extract process noise dimension</span>
0070 Ndim  = InferenceDS.Ndim;                                <span class="comment">% extract observation noise dimension</span>
0071 
0072 N          = ParticleFilterDS.N;                         <span class="comment">% number of particles</span>
0073 particles  = ParticleFilterDS.particles;                 <span class="comment">% copy particle buffer</span>
0074 weights    = ParticleFilterDS.weights;                   <span class="comment">% particle weights</span>
0075 St         = round(InferenceDS.resampleThreshold * N);   <span class="comment">% resample threshold</span>
0076 
0077 onoise = zeros(InferenceDS.Ndim,N);
0078 normWeights = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(1/N,N);
0079 
0080 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0081 
0082 <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,N); <span class="keyword">end</span>
0083 <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,N); <span class="keyword">end</span>
0084 
0085 estimate   = zeros(Xdim,NOV);
0086 
0087 
0088 <span class="keyword">for</span> j=1:NOV,
0089 <span class="comment">%---------------------------------------------------------------------------</span>
0090 
0091     <span class="keyword">if</span> U1dim UU1 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U1(:,j),N); <span class="keyword">end</span>
0092     <span class="keyword">if</span> U2dim UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,j),N); <span class="keyword">end</span>
0093 
0094     processNoise = pNoise.sample( pNoise, N);
0095     particlesPred = InferenceDS.ffun( InferenceDS, particles, processNoise, UU1);
0096 
0097 
0098     <span class="comment">%-----------------------------------------------------------------------</span>
0099     <span class="comment">% EVALUATE IMPORTANCE WEIGHTS</span>
0100 
0101     OBS = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs(:,j),N);
0102 
0103     likelihood = InferenceDS.likelihood( InferenceDS, OBS, particlesPred, UU2, oNoise) + 1e-99;
0104 
0105     weights = weights .* likelihood;
0106     weights = weights / sum(weights);
0107 
0108 
0109 
0110 <span class="keyword">if</span> (0)
0111 prior = InferenceDS.prior( InferenceDS, particlesPred, particles, UU1, pNoise) + 1e-99;
0112 figure(20);
0113 subplot(411)
0114 stem(prior);
0115 ylabel(<span class="string">'prior'</span>);
0116 subplot(412);
0117 stem(likelihood);
0118 ylabel(<span class="string">'likelihood'</span>);
0119 subplot(413);
0120 <span class="comment">%stem(foobuf);</span>
0121 ylabel(<span class="string">'proposal'</span>);
0122 subplot(414);
0123 stem(weights);
0124 ylabel(<span class="string">'weights'</span>);
0125 drawnow
0126 <span class="keyword">end</span>
0127 
0128 
0129     <span class="comment">%-----------------------------------------------------------------------</span>
0130     <span class="comment">% RESAMPLE</span>
0131 
0132     S = 1/sum(weights.^2);     <span class="comment">% calculate effective particle set size</span>
0133 
0134     <span class="keyword">if</span> (S &lt; St)                  <span class="comment">% resample if S is below threshold</span>
0135         outIndex  = <a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>(1:N,weights);
0136         particles = particlesPred(:,outIndex);
0137         weights   = normWeights;
0138     <span class="keyword">else</span>
0139         particles  = particlesPred;
0140     <span class="keyword">end</span>
0141 
0142     <span class="comment">%-----------------------------------------------------------------------</span>
0143     <span class="comment">% CALCULATE ESTIMATE</span>
0144 
0145     <span class="keyword">switch</span> InferenceDS.estimateType
0146 
0147     <span class="keyword">case</span> <span class="string">'mean'</span>
0148         estimate(:,j) = sum(<a href="rvecrep.html" class="code" title="function m = rvecrep(v,c)">rvecrep</a>(weights,Xdim).*particles,2);          <span class="comment">% expected mean</span>
0149 
0150     <span class="keyword">otherwise</span>
0151         error(<span class="string">' [ pf ] Unknown estimate type.'</span>);
0152 
0153     <span class="keyword">end</span>
0154 
0155     <span class="keyword">if</span> pNoise.adaptMethod <span class="keyword">switch</span> InferenceDS.inftype
0156     <span class="comment">%---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------</span>
0157     <span class="keyword">case</span> <span class="string">'parameter'</span>  <span class="comment">%--- parameter estimation</span>
0158         <span class="keyword">switch</span> pNoise.adaptMethod
0159         <span class="keyword">case</span> <span class="string">'anneal'</span>
0160             pNoise.cov = max(pNoise.adaptParams(1) * pNoise.cov , pNoise.adaptParams(2));
0161         <span class="keyword">otherwise</span>
0162             error(<span class="string">' [pf]unknown process noise adaptation method!'</span>);
0163         <span class="keyword">end</span>
0164 
0165     <span class="keyword">case</span> <span class="string">'joint'</span>  <span class="comment">%--- joint estimation</span>
0166         idx = pNoise.idxArr(<span class="keyword">end</span>,:); <span class="comment">% get indexs of parameter block of combo noise source</span>
0167         idxRange = idx(1):idx(2);
0168         <span class="keyword">switch</span> pNoise.adaptMethod
0169         <span class="keyword">case</span> <span class="string">'anneal'</span>
0170             pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
0171         <span class="keyword">otherwise</span>
0172             error(<span class="string">' [pf]unknown process noise adaptation method!'</span>);
0173         <span class="keyword">end</span>
0174         pNoise.noiseSources{pNoise.N}.cov = pNoise.cov(idxRange,idxRange);
0175 
0176     <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0177     <span class="keyword">end</span>; <span class="keyword">end</span>
0178 
0179 
0180 <span class="comment">%--------------------------------------------------------------------------</span>
0181 <span class="keyword">end</span>     <span class="comment">%.. loop over input variables</span>
0182 
0183 
0184 ParticleFilterDS.particles = particles;
0185 ParticleFilterDS.weights   = weights;</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>